#ifndef __CYTOSKELAB_SIM_TK_SOLVER__
#define __CYTOSKELAB_SIM_TK_SOLVER__

#include "SimTKsimbody.h"
#include "simbody/internal/ForceSubsystemGuts.h"

using namespace SimTK;

struct Connection {
  int idx1, idx2;
  const MobilizedBody* body1, *body2;
  Real l;
};

class CytoskelSubsystemImpl : public ForceSubsystem::Guts {

 private:
  mutable DiscreteVariableIndex connectionsIdx;
  
 public:
  
  CytoskelSubsystemImpl();
  Subsystem::Guts* cloneImpl() const;
  int realizeSubsystemTopologyImpl(State& state) const;
  int realizeSubsystemDynamicsImpl(const State& state) const;
  Real calcPotentialEnergy(const State& state) const;
  void setConnections(State&, Array_<Connection>*);
  Array_<Connection>* getConnections(const State&) const;
};

class CytoskelSubsystem : public ForceSubsystem {
public:
  CytoskelSubsystem (MultibodySystem& system);
  
  inline void setConnections(State& state, Array_<Connection>* connections)
  { updImpl().setConnections(state, connections); }

  Array_<Connection>* getConnections(const State& state) const
  { return getImpl().getConnections(state); }

private:
  inline CytoskelSubsystemImpl& updImpl() 
  { return dynamic_cast<CytoskelSubsystemImpl&>(updRep()); }
  
  const inline CytoskelSubsystemImpl& getImpl() const 
  { return dynamic_cast<const CytoskelSubsystemImpl&>(getRep()); }
};

#endif
